/*
 * @Author: fengqiang111 fengqiang@benewake.com
 * @Date: 2022-07-25 11:29:12
 * @LastEditors: xingjian xingjian_liu@yeah.net
 * @LastEditTime: 2023-01-16 11:17:48
 * @FilePath: /preprocess/src/preprocess/src/preprocess_node.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <ros/ros.h>
#include <ros/package.h>
#include <string>
#include <yaml-cpp/yaml.h>
#include "onvifapi/soapH.h"
extern"C" {
    #include "onvifapi/wsaapi.h"
}
#include "onvifapi/wsseapi.h"

#include "ptz_control/absolute_move.h"
#include "ptz_control/continuous_move.h"
#include "ptz_control/goto_preset.h"
#include "ptz_control/stop_move.h"

#include "services/service.hpp"

using namespace ptz;

#define __APP_NAME__ "ptz_node"

#define SOAP_SOCK_TIMEOUT    (10)                                               // socket超时时间（单秒秒）
#define PTZ_CMD_LEFT        (0)
#define PTZ_CMD_RIGHT       (1)
#define PTZ_CMD_UP          (2)
#define PTZ_CMD_DOWN        (3)
#define PTZ_CMD_LEFTUP      (4)
#define PTZ_CMD_LEFTDOWN    (5)
#define PTZ_CMD_RIGHTUP     (6)
#define PTZ_CMD_RIGHTDOWN   (7)
#define PTZ_CMD_ZOOM_IN     (8)
#define PTZ_CMD_ZOOM_OUT    (9)

class PTZNode
{
public:
    PTZNode()
    {
        std::string config_file_path = ros::package::getPath("ptz_control") + "/config/camera_info.yaml";
        YAML::Node config = YAML::LoadFile(config_file_path);

        device_ip_ = config["camera_ip"].as<std::string>();
        username_ = config["camera_username"].as<std::string>();
        password_ = config["camera_passwd"].as<std::string>();
        sprintf( DeviceXAddr, "http://%s/onvif/device_service", device_ip_.c_str() );
    }
    ~PTZNode() {}

    void run()
    {
        // advertise service
        ros::ServiceServer absolute_move_server = nh_.advertiseService(ABSOLUTE_MOVE_SRV, &PTZNode::absoluteMoveCallback, this);
        ros::ServiceServer continuous_move_server = nh_.advertiseService(CONTINUOUS_MOVE_SRV, &PTZNode::continuousMoveCallback, this);
        ros::ServiceServer goto_preset_server = nh_.advertiseService(GOTO_PRESET_SRV, &PTZNode::gotoPresetCallback, this);
        ros::ServiceServer stop_move_server = nh_.advertiseService(STOP_MOVE_SRV, &PTZNode::stopMoveCallback, this);
        
        ros::spin();
    }

private:
    void soap_perror(struct soap *soap, const char *str)
    {
        if (nullptr == str) {
            printf("[soap] error: %d, %s, %s\n", soap->error, *soap_faultcode(soap), *soap_faultstring(soap));
        } else {
            printf("[soap] %s error: %d, %s, %s\n", str, soap->error, *soap_faultcode(soap), *soap_faultstring(soap));
        }
    }

    #define SOAP_CHECK_ERROR(result, soap, str) \
        do { \
            if (SOAP_OK != (result) || SOAP_OK != (soap)->error) { \
                soap_perror((soap), (str)); \
                if (SOAP_OK == (result)) { \
                    (result) = (soap)->error; \
                } \
                goto EXIT; \
            } \
    } while (0)

    void* soapMalloc( struct soap *soap, unsigned int n )
    {
        if ( n > 0 )
        {
            void* p = soap_malloc( soap, n );
            assert(NULL != p);
            memset(p, 0x00 ,n);
            return p;
        }
        return NULL;
    }
    
    struct soap *soapNew( int timeout )
    {
        struct soap *soap = NULL;                                                   // soap环境变量
    
        assert( NULL != (soap = soap_new()) );
    
        soap_set_namespaces(soap, namespaces);                                      // 设置soap的namespaces
        soap->recv_timeout    = timeout;                                            // 设置超时（超过指定时间没有数据就退出）
        soap->send_timeout    = timeout;
        soap->connect_timeout = timeout;
    
    #if defined(__linux__) || defined(__linux)                                      // 参考https://www.genivia.com/dev.html#client-c的修改：
        soap->socket_flags = MSG_NOSIGNAL;                                          // To prevent connection reset errors
    #endif
    
        soap_set_mode(soap, SOAP_C_UTFSTRING);                                      // 设置为UTF-8编码，否则叠加中文OSD会乱码
        return soap;
    }
    
    void soapDelete(struct soap *soap)
    {
        if ( soap == NULL )
            return;
        soap_destroy(soap);                                                         // remove deserialized class instances (C++ only)
        soap_end(soap);                                                             // Clean up deserialized data (except class instances) and temporary data
        soap_done(soap);                                                            // Reset, close communications, and remove callbacks
        soap_free(soap);                                                            // Reset and deallocate the context created with soap_new or soap_copy
    }

    /************************************************************************
    **函数：setAuthInfo
    **功能：设置认证信息
    **参数：
            [in] soap     - soap环境变量
            [in] username - 用户名
            [in] password - 密码
    **返回：
            0表明成功，非0表明失败
    **备注：
    ************************************************************************/
    static int setAuthInfo(struct soap *soap, const char *username, const char *password)
    {
        int result = soap_wsse_add_UsernameTokenDigest( soap, NULL, username, password );
        if ( result != 0 )
            printf("[soap] add_UsernameTokenDigest error: %d, %s, %s\n", result, *soap_faultcode(soap), *soap_faultstring(soap) );
        return result;
    }

    /************************************************************************
    **函数：isH264Profile
    **功能：媒体文件是否264
    **参数：
            [in] addr - media地址
    **返回：
            0表明成功，非0表明失败
    **备注：
    ************************************************************************/
    
    bool isH264Profile( const char* addr, const char* profile )
    {
        struct soap *soap = NULL;
        
        assert( NULL != (soap = soapNew(SOAP_SOCK_TIMEOUT)));
        
        setAuthInfo( soap, username_.c_str(), password_.c_str() );
    
        _trt__GetProfile req;
        _trt__GetProfileResponse resp;
        req.ProfileToken = (char*)profile;
    
        bool bRet = false;
        soap_call___trt__GetProfile( soap, addr, NULL, &req, resp );
        if ( soap->error )
        {
            soap_print_fault( soap, stderr );
        }
        else
        {
            if ( resp.Profile && resp.Profile->VideoEncoderConfiguration 
                && resp.Profile->VideoEncoderConfiguration->Encoding == tt__VideoEncoding__H264 )
                bRet = true;
        }
    
        soapDelete(soap);
        return bRet;
    }

    /************************************************************************
    **函数：getProfiles
    **功能：获得Profiles
    **参数：
            [in] addr - 设备媒体地址
            [out] chProfile -- h264Profile
    **返回：
            0表明成功，非0表明失败
    **备注：
    ************************************************************************/
    
    int getProfiles( const char* addr, char* chProfile )
    {
        struct soap *soap = NULL;
        assert(NULL != (soap = soapNew(SOAP_SOCK_TIMEOUT)));
    
        setAuthInfo(soap, username_.c_str(), password_.c_str());
    
        _trt__GetProfiles req;
        _trt__GetProfilesResponse resp;
    
        soap_call___trt__GetProfiles( soap, addr, NULL, &req, resp );
        if ( soap->error )
        {
            soap_print_fault( soap, stderr );
        }
        else
        {
            int size = resp.__sizeProfiles;
            for ( int i = 0; i < size; ++i )
            {
                if ( resp.Profiles[i] && resp.Profiles[i]->token )
                {
                    char* chToken = resp.Profiles[i]->token;
                    printf( "%s\n", resp.Profiles[i]->token );
                    if ( !isH264Profile( addr, chToken ) )
                        continue;
                    memcpy( chProfile, chToken, strlen( chToken ) );
                    break;
                }
            }
        }
    
        soapDelete(soap);
        return 0;
    }

    /************************************************************************
    **函数：gotoPreset
    **功能：调用指定的预置位
    **参数：
            [in] addr - 设备媒体地址
            [in] profile -- Profile
            [in] iPreset -- 要调用的预置位id
    **返回：
            0表明成功，非0表明失败
    **备注：
    ************************************************************************/
    int gotoPreset( const char *addr, const char* profile, int iPreset )
    {
        struct soap *soap = NULL;
        assert(NULL != (soap = soapNew(SOAP_SOCK_TIMEOUT)));
    
        setAuthInfo(soap, username_.c_str(), password_.c_str());
    
        char chTmp[10];
        sprintf(chTmp, "%d", iPreset );
    
        _tptz__GotoPreset req;
        req.ProfileToken = (char*)profile;
        req.PresetToken = chTmp;
    
        _tptz__GotoPresetResponse resp;
        soap_call___tptz__GotoPreset( soap, addr, NULL, &req, resp );
        if ( soap->error )
        {
            soap_print_fault( soap, stderr );
        }
        else
        {
            printf("call preset %d is ok\n", iPreset );
        }
        soapDelete(soap);
        return 0;
    }

    // 以指定速度移动到指定位置的ptz
    // p :  -1  ~ 1   []
    // t :  -1  ~ 1
    // z :   0  ~ 1
    int absoluteMove(const std::string& addr, const std::string& profile, float pan, float titl, float zoom){
        int result = 0;
        struct soap *soap = NULL;
        _tptz__AbsoluteMove           absoluteMove;
        _tptz__AbsoluteMoveResponse   absoluteMoveResponse;

        assert(!addr.empty() && !profile.empty());
        assert(NULL != (soap = soapNew(SOAP_SOCK_TIMEOUT)));


        setAuthInfo(soap, username_.c_str(), password_.c_str());
        absoluteMove.ProfileToken = const_cast<char *>(profile.c_str());

        absoluteMove.Position = soap_new_tt__PTZVector(soap);
        absoluteMove.Position->PanTilt = soap_new_tt__Vector2D(soap);
        absoluteMove.Position->Zoom = soap_new_tt__Vector1D(soap);
        absoluteMove.Speed = soap_new_tt__PTZSpeed(soap);
        absoluteMove.Speed->PanTilt = soap_new_tt__Vector2D(soap);
        absoluteMove.Speed->Zoom = soap_new_tt__Vector1D(soap);


        absoluteMove.Position->PanTilt->x = pan;  // p
        absoluteMove.Position->PanTilt->y = titl;   // t
        absoluteMove.Position->Zoom->x = zoom;   // z

        // x 和y的绝对值越接近1，表示云台的速度越快
        absoluteMove.Speed->PanTilt->x = 0.5;
        absoluteMove.Speed->PanTilt->y = 0.5;
        absoluteMove.Speed->Zoom->x = 0.5;
        result = soap_call___tptz__AbsoluteMove(soap, addr.c_str(), NULL,&absoluteMove,absoluteMoveResponse);
        SOAP_CHECK_ERROR(result, soap, "ONVIF_PTZAbsoluteMove");
    EXIT:
        if (NULL != soap) {
            soapDelete(soap);
        }
        return 0;
    }

    // speed --> (0, 1]
    int continuousMove(const std::string& addr, const std::string& profile, int cmd, float speed){
        int result = 0;
        struct soap *soap = NULL;
        _tptz__ContinuousMove continuousMove;
        _tptz__ContinuousMoveResponse continuousMoveResponse;

        assert(!addr.empty() && !profile.empty());
        assert(NULL != (soap = soapNew(SOAP_SOCK_TIMEOUT)));

        setAuthInfo(soap, username_.c_str(), password_.c_str());
        continuousMove.ProfileToken = const_cast<char *>(profile.c_str());
        continuousMove.Velocity = soap_new_tt__PTZSpeed(soap);
        continuousMove.Velocity->PanTilt = soap_new_tt__Vector2D(soap);
        continuousMove.Velocity->Zoom = soap_new_tt__Vector1D(soap);
        switch (cmd)
        {
            case  PTZ_CMD_LEFT:
                continuousMove.Velocity->PanTilt->x = -speed;
                continuousMove.Velocity->PanTilt->y = 0;
                break;
            case  PTZ_CMD_RIGHT:
                continuousMove.Velocity->PanTilt->x = speed;
                continuousMove.Velocity->PanTilt->y = 0;
                break;
            case  PTZ_CMD_UP:
                continuousMove.Velocity->PanTilt->x = 0;
                continuousMove.Velocity->PanTilt->y = speed;
                break;
            case  PTZ_CMD_DOWN:
                continuousMove.Velocity->PanTilt->x = 0;
                continuousMove.Velocity->PanTilt->y = -speed;
                break;
            case  PTZ_CMD_LEFTUP:
                continuousMove.Velocity->PanTilt->x = -speed;
                continuousMove.Velocity->PanTilt->y = speed;
                break;
            case PTZ_CMD_LEFTDOWN:
                continuousMove.Velocity->PanTilt->x = -speed;
                continuousMove.Velocity->PanTilt->y = -speed;
                break;
            case  PTZ_CMD_RIGHTUP:
                continuousMove.Velocity->PanTilt->x = speed;
                continuousMove.Velocity->PanTilt->y = speed;
                break;
            case PTZ_CMD_RIGHTDOWN:
                continuousMove.Velocity->PanTilt->x = speed;
                continuousMove.Velocity->PanTilt->y = -speed;
                break;
            case  PTZ_CMD_ZOOM_IN:
                continuousMove.Velocity->PanTilt->x = 0;
                continuousMove.Velocity->PanTilt->y = 0;
                continuousMove.Velocity->Zoom->x = speed;
                break;
            case  PTZ_CMD_ZOOM_OUT:
                continuousMove.Velocity->PanTilt->x = 0;
                continuousMove.Velocity->PanTilt->y = 0;
                continuousMove.Velocity->Zoom->x = -speed;
                break;
            default:
                break;
        }
        result = soap_call___tptz__ContinuousMove(soap, addr.c_str(), NULL, &continuousMove,continuousMoveResponse);
        SOAP_CHECK_ERROR(result, soap, "ONVIF_PTZContinuousMove");
    EXIT:
        if (NULL != soap) {
            soapDelete(soap);
        }
        return result;
    }

    int stopMove(const std::string& addr, const std::string& profile){
        int result = 0;
        struct soap *soap = NULL;
        _tptz__Stop tptzStop;
        _tptz__StopResponse tptzStopResponse;

                assert(!addr.empty() && !profile.empty());
                assert(NULL != (soap = soapNew(SOAP_SOCK_TIMEOUT)));

        setAuthInfo(soap, username_.c_str(), password_.c_str());
        tptzStop.ProfileToken = const_cast<char *>(profile.c_str());
        result = soap_call___tptz__Stop(soap, addr.c_str(), NULL, &tptzStop, tptzStopResponse);
        SOAP_CHECK_ERROR(result, soap, "ONVIF_PTZStopMove");

        EXIT:
        if (NULL != soap) {
            soapDelete(soap);
        }
        return result;
    }

    bool getServices(std::string &m_szPtzAddr, std::string &m_szProfile)
    {
        struct soap *soap = NULL;
    
        assert(NULL != ( soap = soapNew( SOAP_SOCK_TIMEOUT ) ) );
        //用户鉴权
        setAuthInfo(soap, username_.c_str(), password_.c_str());
    
        _tds__GetServices tds_req;
        tds_req.IncludeCapability = true;
        _tds__GetServicesResponse tds_resp;
    
        soap_call___tds__GetServices( soap,  DeviceXAddr, NULL, &tds_req, tds_resp );
        if ( soap->error )
        {
            soap_print_fault( soap, stderr );
            soapDelete(soap);
            return false;
        }
        else
        {
            int size = tds_resp.__sizeService;
            for ( int i = 0; i < size; ++i )
            {
                if ( tds_resp.Service[i] && tds_resp.Service[i]->XAddr )
                {
                    char* pTmp = tds_resp.Service[i]->XAddr;
                    printf( "%s\n", tds_resp.Service[i]->XAddr );
    
                    std::string addr = pTmp;
    
                    if ( addr.find( "media" ) != std::string::npos
                        || addr.find( "Media" ) != std::string::npos )
                    {					
                        char chProfile[100] = {0};
                        getProfiles( pTmp, chProfile );
                        m_szProfile = std::string( chProfile );
                    }
                    else if ( addr.find( "ptz" ) != std::string::npos 
                        || addr.find( "PTZ" ) != std::string::npos )
                    {
                        m_szPtzAddr = addr;
                    }
                }
            }
            return true;
        }
    }

    bool gotoPresetCallback(ptz_control::goto_preset::Request &req, ptz_control::goto_preset::Response &res)
    {
        std::string m_szPtzAddr;
		std::string m_szProfile;
        if(getServices(m_szPtzAddr, m_szProfile))
        {
            gotoPreset( m_szPtzAddr.c_str(), m_szProfile.c_str(), req.preset_id );
            res.success = true;
            return true;
        }
        res.success = false;
        return false;
    }

    bool absoluteMoveCallback(ptz_control::absolute_move::Request &req, ptz_control::absolute_move::Response &res)
    {
        std::string m_szPtzAddr;
		std::string m_szProfile;
        if(getServices(m_szPtzAddr, m_szProfile))
        {
            absoluteMove( m_szPtzAddr.c_str(), m_szProfile.c_str(), req.pan, req.titl, req.zoom );
            res.success = true;
            return true;
        }
        res.success = false;
        return false;
    }

    bool continuousMoveCallback(ptz_control::continuous_move::Request &req, ptz_control::continuous_move::Response &res)
    {
        std::string m_szPtzAddr;
		std::string m_szProfile;
        if(getServices(m_szPtzAddr, m_szProfile))
        {
            continuousMove( m_szPtzAddr.c_str(), m_szProfile.c_str(), req.cmd, req.speed);
            res.success = true;
            return true;
        }
        res.success = false;
        return false;
    }

    bool stopMoveCallback(ptz_control::stop_move::Request &req, ptz_control::stop_move::Response &res)
    {
        std::string m_szPtzAddr;
		std::string m_szProfile;
        if(getServices(m_szPtzAddr, m_szProfile))
        {
            stopMove( m_szPtzAddr.c_str(), m_szProfile.c_str() );
            res.success = true;
            return true;
        }
        res.success = false;
        return false;
    }

    std::string device_ip_;
    std::string username_;
    std::string password_;
    char DeviceXAddr[100] = { 0 };
    ros::NodeHandle nh_;
};

int main(int argc, char *argv[])
{
    ros::init(argc, argv, __APP_NAME__);
    PTZNode ptz_node;
    ptz_node.run();
    return 0;
}
